// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectCameraCalibration.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Cameras
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Runtime.Serialization;
    using System.Text;
    using System.Xml;

    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Computes transformations between image and world coordinates for
    /// kinect cameras using intrinsic and extrinsic calibrations
    /// </summary>
    [DataContract]
    public class KinectCameraCalibration
    {
        /// <summary>
        /// The maximum projection component of camera X or Y into world coordinates
        /// </summary>
        private double maxProjection;

        /// <summary>
        /// Initializes a new instance of the <see cref="KinectCameraCalibration" /> class
        /// </summary>
        /// <param name="intrinsicMatrix">Intrinsic camera matrix</param>
        /// <param name="extrinsicPose">Extrinsic camera pose</param>
        public KinectCameraCalibration(Matrix3 intrinsicMatrix, Pose extrinsicPose)
        {
            this.IntrinsicMatrix = intrinsicMatrix;
            this.ExtrinsicMatrix = HomogenousTransform.CreateFromPose(extrinsicPose);

            this.Initialize();
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="KinectCameraCalibration" /> class
        /// </summary>
        protected KinectCameraCalibration()
        {
            this.IntrinsicMatrix = new Matrix3();
            this.ExtrinsicMatrix = new Matrix4();
        }

        /// <summary>
        /// Gets camera intrinsic calibration matrix
        /// </summary>
        [DataMember]
        public Matrix3 IntrinsicMatrix { get; private set; }

        /// <summary>
        /// Gets Extrinsic matrix 
        /// </summary>
        [DataMember]
        public Matrix4 ExtrinsicMatrix { get; private set; }

        /// <summary>
        /// Gets the Inverse Extrinsic matrix
        /// </summary>
        [DataMember]
        public Matrix4 ExtrinsicMatrixInv { get; private set; }

        /// <summary>
        /// Gets inverse K
        /// </summary>
        public Matrix3 IntrinsicMatrixInv { get; private set; }

        /// <summary>
        /// Gets the focal length X
        /// </summary>
        public double FocalX
        {
            get
            {
                return Math.Abs(this.IntrinsicMatrix[0, 0]);
            }
        }

        /// <summary>
        /// Gets the focal length Y
        /// </summary>
        public double FocalY
        {
            get
            {
                return Math.Abs(this.IntrinsicMatrix[1, 1]);
            }
        }

        /// <summary>
        /// Gets the camera center X
        /// </summary>
        public double CameraCenterX
        {
            get
            {
                return this.IntrinsicMatrix[0, 2];
            }
        }

        /// <summary>
        /// Gets the camera center X
        /// </summary>
        public double CameraCenterY
        {
            get
            {
                return this.IntrinsicMatrix[1, 2];
            }
        }

        /// <summary>
        /// Gets pixel coordinates from external position
        /// </summary>
        /// <param name="extrinsicPositionInM">Extrinsic position in meter</param>
        /// <returns>Location in pixels. Null if cannot compute</returns>
        public Vector3? PixelCordsFromPosition(Vector3? extrinsicPositionInM)
        {
            if (true == extrinsicPositionInM.HasValue)
            {
                Vector4 posh = new Vector4(extrinsicPositionInM.Value);
                
                Vector4 v = this.ExtrinsicMatrix * posh;

                double z = 0.001;
                if (Math.Abs(v.Z) > 0.0)
                {
                    z = Math.Abs(v.Z);
                }

                Vector3 v3 = new Vector3(v.X / z, v.Y / z, 1.0);
                Vector3 v4 = this.IntrinsicMatrix * v3;
                v4.Z = z;

                return v4;
            }

            return null;
        }

        /// <summary>
        /// Converts point in image (pixels) to camera centered world coordinates  in meter. 
        /// Note Depth is in meter
        /// </summary>
        /// <param name="imageIn">Image position (column (x), row (y) and  depth (z) in meter)</param>
        /// <returns>Camera World position in meter. null if cannot be computed</returns>
        public Vector3? ImageToCameraWorldConvert(Vector3? imageIn)
        {
            Vector3? world = null;

            if (true == imageIn.HasValue)
            {
                Vector3 image = imageIn.Value;
                Vector3 imageN = new Vector3(image.X, image.Y, 1);
                Vector3 t = this.IntrinsicMatrixInv * imageN;
                t *= image.Z;
                world = (Vector3?)t;
            }

            return world;
        }

        /// <summary>
        /// Converts point in image (pixel) to world coordinates (meters). World coordinates here 
        /// means absolute centered world coordinate.
        /// </summary>
        /// <param name="imageIn">Image position  (column (x), row (y) and  depth (z) in meter)</param>
        /// <returns>world position in meter. null if cannot compute</returns>
        public Vector3? ImageToWorldConvert(Vector3? imageIn)
        {
            Vector3? camera = this.ImageToCameraWorldConvert(imageIn);
            Vector3? world = null;

            if (camera.HasValue)
            {
                Vector4 camera4 = new Vector4(camera.Value);
                Vector4 w4 = this.ExtrinsicMatrixInv * camera4;

                world = new Vector3(w4.X, w4.Y, w4.Z);
            }

            return world;
        }

        /// <summary>
        /// Return the projected camera patch size in pixels for a solid object located in the camera frame
        /// </summary>
        /// <param name="pixX">Object X center in camera pixels</param>
        /// <param name="pixY">Object Y center in camera pixels</param>
        /// <param name="cameraZInM">Object center distance in meter from camera</param>
        /// <param name="widthInM">Object width measured in meter</param>
        /// <param name="heightInM">Object height measured in meter</param>
        /// <param name="depthInM">Object depth measured in meter</param>
        /// <param name="patchScale">Scale factor for output patch (normally 1.0)</param>
        /// <param name="patchXPix">Output X patch size in pixels</param>
        /// <param name="patchYPix">Output Y patch size in pixels</param>
        /// <param name="patchZPix">Output Z patch size in meter</param>
        public void GetPatchSizeInPixForObject(double pixX, double pixY, double cameraZInM, double widthInM, double heightInM, double depthInM, double patchScale, out double patchXPix, out double patchYPix, out double patchZPix)
        {
            ////Vector3 pixCenter = new Vector3(pixX, pixY, cameraZInM * MathConstants.MilliunitsPerUnit);
            Vector3 pixCenter = new Vector3(pixX, pixY, cameraZInM);
            Vector3 worldCenter = this.ImageToWorldConvert(pixCenter).Value;

            double widthBy2 = (widthInM / 2.0);
            double depthBy2 = depthInM / 2.0;
            double heightBy2 = heightInM / 2.0;

            Vector3? minPixels = null;
            Vector3? maxPixels = null;

            Vector3[] extrema = new Vector3[] { new Vector3(widthBy2 * this.maxProjection, depthBy2 * this.maxProjection, heightBy2), new Vector3(-widthBy2 * this.maxProjection, -depthBy2 * this.maxProjection, -heightBy2) };

            for (int i = 0; i < extrema.Length; ++i)
            {
                for (int j = 0; j < extrema.Length; ++j)
                {
                    for (int k = 0; k < extrema.Length; ++k)
                    {
                        Vector3 pert = new Vector3(extrema[i].X, extrema[j].Y, extrema[k].Z);
                        this.UpdateMinMaxInPixs(ref minPixels, ref maxPixels, worldCenter + pert);
                    }
                }
            }

            patchXPix = (maxPixels.Value.X - minPixels.Value.X) * patchScale;
            patchYPix = (maxPixels.Value.Y - minPixels.Value.Y) * patchScale;
            patchZPix = (maxPixels.Value.Z - minPixels.Value.Z) * patchScale;
        }

        /// <summary>
        /// Maps a depth image (column, row) to the corresponding (column row) in the RGB image
        /// </summary>
        /// <param name="depth2RGBmap">depth to RGB map</param>
        /// <param name="rgbFRame">Target RGB frame</param>
        /// <param name="depthColPix">Depth image column to map</param>
        /// <param name="depthRowPix">Depth image row to map</param>
        /// <param name="rgbColpix">Mapped RGB column</param>
        /// <param name="rgbRowPix">Mapped RGB row </param>
        /// <returns>True if the map is valid false otherwise</returns>
        public bool MapDepthToRGB(ImageFrame<Vector2> depth2RGBmap, ImageFrameRgb rgbFRame, double depthColPix, double depthRowPix, out double rgbColpix, out double rgbRowPix)
        {
            bool isValid = false;
            rgbColpix = 0.0;
            rgbRowPix = 0.0;

            if (null != depth2RGBmap && depthColPix >= 0 && depthColPix < depth2RGBmap.Width && depthRowPix >= 0 && depthRowPix < depth2RGBmap.Height)
            {
                Vector2 mapped = depth2RGBmap[(int)(depthColPix + 0.5), (int)(depthRowPix + 0.5)];
                if (mapped.X >= 0 && (int)(mapped.X) < rgbFRame.Width)
                {
                    rgbColpix = mapped.X;

                    if (mapped.Y >= 0 && (int)(mapped.Y) < rgbFRame.Height)
                    {
                        rgbRowPix = mapped.Y;
                        isValid = true;
                    }
                }
            }

            return isValid;
        }

        /// <summary>
        /// Update minimum and maximum accumulators in pixel coordinates
        /// </summary>
        /// <param name="minCamera">Minimum accumulator in camera pixels</param>
        /// <param name="maxCamera">Maximum accumulator in camera pixels</param>
        /// <param name="worldExtreme">Extreme point in world coordinates</param>
        protected void UpdateMinMaxInPixs(ref Vector3? minCamera, ref Vector3? maxCamera, Vector3 worldExtreme)
        {
            Vector3 pixelExtreme = this.PixelCordsFromPosition(worldExtreme).Value;

            if (false == minCamera.HasValue)
            {
                minCamera = pixelExtreme;
            }
            else
            {
                minCamera = new Vector3(
                            Math.Min(minCamera.Value.X, pixelExtreme.X),
                            Math.Min(minCamera.Value.Y, pixelExtreme.Y),
                            Math.Min(minCamera.Value.Z, pixelExtreme.Z));
            }

            if (false == maxCamera.HasValue)
            {
                maxCamera = pixelExtreme;
            }
            else
            {
                maxCamera = new Vector3(
                            Math.Max(maxCamera.Value.X, pixelExtreme.X),
                            Math.Max(maxCamera.Value.Y, pixelExtreme.Y),
                            Math.Max(maxCamera.Value.Z, pixelExtreme.Z));
            }
        }

        /// <summary>
        /// Initialize inverse of the calibration matrices
        /// </summary>
        protected void Initialize()
        {
            this.IntrinsicMatrixInv = this.IntrinsicMatrix.Inverse();
            this.ExtrinsicMatrixInv = Matrix4.Inverse(this.ExtrinsicMatrix);
            Vector3 projX = this.WorldXYUnitVectorFromCameraDirection(new Vector3(this.CameraCenterX + 100.0, this.CameraCenterY, 1000));

            // Choose the maximum component of the projection from the camera of X (columns) or Y (rows). This is used to estimate the largest patch
            this.maxProjection = Math.Max(Math.Abs(projX.X), Math.Abs(projX.Y));
        }

        /// <summary>
        /// Construct a unit vector projected onto XY in world coordinates corresponding to 
        /// a direction in camera coordinates
        /// </summary>
        /// <param name="cameraDirection">Input camera direction</param>
        /// <returns>Unit vector in world coordinates</returns>
        private Vector3 WorldXYUnitVectorFromCameraDirection(Vector3 cameraDirection)
        {
            Vector3 origin = new Vector3(this.CameraCenterX, this.CameraCenterY, cameraDirection.Z);
            Vector3 originWorld = this.ImageToWorldConvert(origin).Value;
            Vector3 world2 = this.ImageToWorldConvert(cameraDirection).Value;
            Vector3 mapped = world2 - originWorld;
            mapped.Z = 0.0;

            return Vector3.Normalize(mapped);
        }
    }
}
